#include "api.h"
#include <map>
#include "nettransmission.h"
#include <unistd.h>
#include <iostream>
#include "Params.h"
#include "UtilityTools.h"
#include "ConvPCD.h"
#ifdef PCL
#include "ConvPCL.h"
#endif //PCL
static ERROR_CODE inside_error_code = STATUS_SUCCESS;

struct STRC_DEV{
    NetTransmission *ctl;   //控件通道
    NetTransmission *img;   //图像传输通道
    CParams *params;             //参数
    char *buffPCD;
};

static map<int , STRC_DEV*> mMapDev;
static int mIndexDev = 0;






EXPORT void api_init()
{
    mIndexDev = 0;
}


EXPORT void api_exit()
{
    if(mMapDev.size() != 0)
    {
        map<int , STRC_DEV*>::iterator item;
        for(item = mMapDev.begin() ; item != mMapDev.end(); item++)
        {
            STRC_DEV *pDev = (STRC_DEV *)item->second;
            pDev->ctl->Stop();
            delete pDev->ctl;
            pDev->img->Stop();
            delete pDev->img;
            delete pDev->params;
            if(pDev->buffPCD != NULL)
            {
                free(pDev->buffPCD);
            }
            free(pDev);
        }
        mMapDev.clear();
        mIndexDev = 0;
    }
}

EXPORT ERROR_CODE api_errorcode()
{
    return inside_error_code;
}

EXPORT void api_set_errorcode(ERROR_CODE code)
{
    inside_error_code = code;
}

EXPORT int api_connect(char *ip , int port)
{
    int mDev = 0;
    STRC_DEV *pDev = (STRC_DEV *)malloc(sizeof(STRC_DEV));
    pDev->ctl = new NetTransmission();
    if(pDev->ctl->Start(string(ip) , port, NetTransmission::PIPE_CONTROL) >=0)
    {
        pDev->params = new CParams();
        pDev->img = new NetTransmission();
        pDev->buffPCD = (char *)malloc(MAX_LEN_PCB);
        if (pDev->img->Start(string(ip), port, NetTransmission::PIPE_IMG_SEPARATE) >= 0)
        {
            mIndexDev++;
            mDev = mIndexDev;
            mMapDev.insert({mDev , pDev});
        }else{
            mDev = -1;
            pDev->ctl->Stop();
            delete pDev->img;
            delete pDev->ctl;
            if (pDev->buffPCD != NULL)
            {
                free(pDev->buffPCD);
                pDev->buffPCD = NULL;
            }
            free(pDev);           
        }
        
    }else{
        mDev = -1;
        delete pDev->ctl;
        free(pDev);
    }
    

    return mDev;
}

EXPORT int api_disconnect(int handle)
{
    int ret = 0;
    auto item = mMapDev.find(handle);
    if(item == mMapDev.end())
    {
        ret = -1;
    }else{
        STRC_DEV *pDev = item->second;
        pDev->img->Stop();
        delete pDev->img;
        pDev->ctl->Stop();
        delete pDev->ctl;
        delete pDev->params;
        free(pDev);
        mMapDev.erase(handle);
    }
    return ret;
}

EXPORT char *api_get_sdk_ver()
{
    return (char *)VER;
}

EXPORT char *api_get_dev_ver(int handle)
{
    auto item = mMapDev.find(handle);
    if(item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        return pDev->ctl->GetSdkVer();
    }

    return NULL;
}

EXPORT char *api_get_intrinsic_parameters(int handle)
{
    auto item = mMapDev.find(handle);
    if(item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        return pDev->ctl->GetLensInfo();
    }

    return NULL;    
}

EXPORT STRC_IMG_ALL *api_get_img(int handle )
{
    STRC_IMG_ALL *pImg = NULL;
    auto item = mMapDev.find(handle);
    if(item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        pImg = pDev->img->GetImg();
        if(pImg != NULL && pImg->img_depth.len > 0)
        {
#ifdef PCL        
            if(pImg->pCloud == NULL)
            {
                pImg->pCloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            }
            ConverToPCL(*pImg, api_get_intrinsic_parameters(handle));
#endif //PCL
            GammaCorrection(pImg , pDev->params , pDev->ctl->GetParams());
        }
    }
    return pImg;
}

EXPORT int api_get_exposure(int handle , int *min , int *max)
{
    auto item = mMapDev.find(handle);
    if(item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        return pDev->ctl->GetExposure(min , max);
    }
    return -1;
}

EXPORT int api_set_exposure(int handle , int exposure)
{
    auto item = mMapDev.find(handle);
    if(item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        return pDev->ctl->SetExposure(exposure);
    }
    return -1;

}

EXPORT int api_set_gamma_amplitude(int handle, float gama, bool isActive)
{
    auto item = mMapDev.find(handle);
    if (item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        pDev->params->m_GammaAmplitude = gama;
        pDev->params->m_bUseGammaAmplitude = isActive;
        return 0;
    }
    return -1;
}

EXPORT char *api_get_sn_tof(int handle)
{
    auto item = mMapDev.find(handle);
    if (item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        return pDev->ctl->GetSNTof();
    }

    return NULL;
}

EXPORT int api_set_xin1(int handle, int num_frames)
{
    int ret = -1;
    auto item = mMapDev.find(handle);
    if (item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        return pDev->ctl->SetFrameSuperposition(num_frames);
    }

    return ret;
}

EXPORT int api_set_blocking_mode(int handle, bool blockingActive)
{
    int ret = -1;
    auto item = mMapDev.find(handle);
    if (item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        pDev->ctl->SetBlockingActive(blockingActive);
        pDev->img->SetBlockingActive(blockingActive);
        ret = 0;
    }
    return ret;
}

EXPORT int api_depth2pcd(int handle, uint16_t *depth, char **pcd_data)
{
    int ret = -1;
    auto item = mMapDev.find(handle);
    if (item != mMapDev.end())
    {
        STRC_DEV *pDev = item->second;
        *pcd_data = pDev->buffPCD;
        ret = Depth2PCD(depth, pDev->buffPCD, api_get_intrinsic_parameters(handle));
    }
    return ret;
}

EXPORT int api_get_rgb_valid(int handle, bool *rgb_valid)
{
    int ret = -1;
    if (api_get_intrinsic_parameters(handle) != NULL)
    {
        auto item = mMapDev.find(handle);
        if (item != mMapDev.end())
        {
            STRC_DEV *pDev = item->second;
            StrcParam DevParam = pDev->ctl->GetParams();
            *rgb_valid = DevParam.RgbValid == 1 ? true : false;
            ret = 0;
        }
    } else
    {
        api_set_errorcode(STATUS_HANDLE_FAILED);
    }

    return ret;
}